#include "lidar.hpp"

#include <utility>

#include "spdlog/spdlog.h"

#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))

namespace {

const int kSAFE = 150;
const float kMIN = 174.99;
const float kMAX = 360 - kMIN;

}  // namespace

bool Lidar::Init() {
  // signal_.Init();
  scanning_ = false;

  SPDLOG_WARN("Port name : {}", usb_.ConnectPortName(USBDevice::kDEVICE_LIDAR));

  sl_lidar_response_device_info_t dev_info;
  sl_lidar_response_device_health_t health_info;
  sl_result result;

  driver_ = *sl::createLidarDriver();
  if (!driver_) {
    SPDLOG_ERROR("Can't create lidar driver");
    return false;
  }

  sl::IChannel *channel =
      *sl::createSerialPortChannel(usb_.GetPortName(), 1000000);
  if (SL_IS_OK(driver_->connect(channel))) {
    result = driver_->getDeviceInfo(dev_info);
    if (!SL_IS_OK(result)) {
      SPDLOG_ERROR("Can't connect lidar, check usb condition {}",
                   usb_.GetPortName());
      return false;
    } else {
      SPDLOG_INFO("Device Info : Firmware Ver : {}.{}, Hardware Ver : {}",
                  (int)dev_info.firmware_version >> 8,
                  (int)dev_info.firmware_version & 0xFF,
                  (int)dev_info.hardware_version);
    }
  }

  result = driver_->getHealth(health_info);
  if (SL_IS_OK(result)) {
    SPDLOG_INFO("Lidar health code : {}", health_info.status);
    if (health_info.status == SL_LIDAR_STATUS_ERROR) {
      SPDLOG_ERROR("Device need reboot, please restart the app");
      driver_->reset();
      return false;
    }
  } else {
    SPDLOG_ERROR("Can't retrieve health code : {}", result);
    return false;
  }

  driver_->setMotorSpeed();
  driver_->startScan(0, 1);
  scanning_ = true;

  SPDLOG_WARN("Inited Lidar.");
  return true;
}

void Lidar::ThreadScan() {
  std::vector<component::LidarPoint> points;
  sl_lidar_response_measurement_node_hq_t nodes[4096];

  while (scanning_) {
    points.clear();

    size_t count = _countof(nodes);
    points.resize(count);

    auto result = driver_->grabScanDataHq(nodes, count);
    if (SL_IS_OK(result)) {
      driver_->ascendScanData(nodes, count);
      for (auto &node : nodes) {
        component::LidarPoint pt(node.angle_z_q14 * 90.f / 16384.f,
                                 node.dist_mm_q2 / 40.f);
        points.emplace_back(pt);
        SPDLOG_DEBUG("angle {}, dis {}", pt.angle_, pt.dist_);
      }
    } else {
      SPDLOG_ERROR("Can't grab scan data");
      // for (auto &p : points) {
      //   SPDLOG_WARN("{},{}", p.angle_, p.dist_);
      // }
    }

    std::lock_guard<std::mutex> lock(mutex_);
    datas_.emplace_back(points);
    // signal_.Signal();
    SPDLOG_WARN("Circled");
  }
}

Lidar::Lidar() { SPDLOG_TRACE("Constructed."); }

Lidar::~Lidar() {
  delete driver_;
  driver_ = nullptr;
  thread_lidar_.join();
  SPDLOG_TRACE("Destructed.");
}

bool Lidar::Open() {
  Init();
  SPDLOG_WARN("[ThreadScan] Start");
  if (scanning_) {
    thread_lidar_ = std::thread(&Lidar::ThreadScan, this);
    return true;
  }
  return false;
}

bool Lidar::Disconnect() {
  driver_->disconnect();
  return true;
}

// int Lidar::GetFrequency() { return driver_->getFrequency() }
std::vector<component::LidarPoint> Lidar::GetData() {
  // signal_.Wait();
  std::vector<component::LidarPoint> data;
  std::lock_guard<std::mutex> lock(mutex_);
  if (!datas_.empty()) {
    data = datas_.back();
    if (datas_.size() > 50) datas_.clear();

    if (data.size() > 50) {
      // SPDLOG_WARN("TRUE");
      return data;
    }
  }

  // data.clear();
  return data;
}

// bool Lidar::GetData(std::vector<component::LidarPoint> &node) {
//   if (!datas_.empty()) {
//     node = datas_.back();
//     if (datas_.size() > 50) datas_.clear();

//     if (node.size() > 50) {
//       SPDLOG_WARN("TRUE");
//       return false;
//     } else {
//       SPDLOG_WARN(" A FALSE");
//       return false;
//     }
//   }
//   SPDLOG_WARN("B FALSE");
//   return false;
// }

bool Lidar::Stop() {
  scanning_ = false;
  driver_->setMotorSpeed(0);
  SPDLOG_DEBUG("Stop scanning");
  return true;
}

/////////////////////////////////////////////
// LidarDriver
/////////////////////////////////////////////

bool LidarDriver::CalObstrcts(std::vector<component::LidarPoint> points) {
  inners_.clear();
  measured_.clear();
  float temp[720] = {0.f};

  if (points.size() <= 1) return false;
  for (auto &point : points) {
    if (point.angle_ >= min_ && point.angle_ <= max_) {
      SPDLOG_DEBUG("{} IN LIMIT", point.angle_);
      continue;
    }
    if (int(point.dist_) == 0) {
      SPDLOG_DEBUG("DIS = 0");
      continue;
    }
    if (point.dist_ > kSAFE) {
      SPDLOG_DEBUG("dist {} > {}", point.dist_, kSAFE);
      continue;
    };

    inners_.emplace_back(point);
  }
  if (inners_.size() < 2) {
    // SPDLOG_WARN("size < 3");
    return false;
  }
  std::sort(
      inners_.begin(), inners_.end(),
      [](const component::LidarPoint &p1, const component::LidarPoint &p2) {
        return p1.angle_ < p2.angle_;
      });
  SPDLOG_WARN("---------------------[Size : {}]---------------",
              inners_.size());

  if (inners_.size() < 2) return false;

  std::size_t elem_idx = 0;
  int angle_index = 0;
  float min = inners_[0].dist_;
  for (; elem_idx < inners_.size(); elem_idx++) {
    while (angle_index < int(inners_[elem_idx].angle_ * 2)) {
      angle_index += 1;
      min = inners_[elem_idx].dist_;
      // SPDLOG_WARN("index {}, {}", angle_index, min);
    }
    temp[angle_index] = std::min(min, inners_[elem_idx + 1].dist_);
    // SPDLOG_INFO("Now : e_{}, e_r{}, a_{}, min {} ", elem_idx,
    //             inners_[elem_idx].dist_, angle_index, min);
  }

  if (measured_.size() == 0) measured_.resize(720, 0);
  for (size_t i = 0; i < measured_.size(); i++) {
    measured_[i] = temp[i];
  }
  return true;
}

LidarDriver::LidarDriver() {
  min_ = kMIN;
  max_ = kMAX;
  SPDLOG_TRACE("Constructed.");
}

LidarDriver::LidarDriver(double a, double b) {
  min_ = a;
  max_ = b;
  if (min_ > max_) std::swap(min_, max_);
  lidar_.Open();
  SPDLOG_TRACE("Constructed.");
}

LidarDriver::~LidarDriver() {
  lidar_.~Lidar();
  SPDLOG_TRACE("Destructed.");
}

bool LidarDriver::Init() { return lidar_.Open(); }

void LidarDriver::SetLimit(double a, double b) {
  min_ = a;
  max_ = b;
  if (min_ > max_) std::swap(min_, max_);
  SPDLOG_INFO("min : {}, max : {}", min_, max_);
}

std::vector<float> LidarDriver::Apply() {
  auto points = lidar_.GetData();
 
  CalObstrcts(points);
 
  return measured_;
}

std::vector<uint8_t> LidarDriver::GetResult() {
  std::vector<uint8_t> result;
  if (measured_.size() > 0) {
    for (auto i = 0; i < measured_.size(); i++)
      result.emplace_back(static_cast<uint8_t>(measured_[i]));
  }
  return result;
}
